/**
*  \file kwpservices.c
*
*  \brief
*
*  \author Michael J. Hohmann  <michael.hohmann@linde-mh.de>
*
*
*/

/*===============================================[ private includes  ]================================================*/
#include "../kwpservices/kwpservices.h"
#include "../assertor/assertor.h"
#include "../endian/endian_read.h"
#include "../seppuku/seppuku.h"
#include "../secacc/secacc.h"
#include "../deaddrop/deaddrop.h"
#include "../cancom/cancom.h"
#include "../nvdata/nvdata.h"
#include "../nvram/nvram.h"
#include "../parameter/parameter.h"
#include "../systime/systime.h"
#include "../endian/endian_write.h"
#include "../swidft/swidft.h"
#include "../mcu/mcu.h"
#include "../common/commdef.h"
#include "../paragen/paragen.h"
#include "../dtc/dtc.h"
#include "../parameter/parameter.h"
#include "../term/term.h"
/*===============================================[ private defines   ]================================================*/

/*===============================================[ private datatypes ]================================================*/

/** Private variables */
struct kwpservices_private
{
//   enum kwpservices_diagnostic_sesssion session_type;
   BO passive_mode;
   systime_t test_timer;
};


/*===============================================[ private functions ]================================================*/

static enum kwp_response_code start_diagnostic_session                   ( struct kwp_msg* msg ); /* 0x10 */
static enum kwp_response_code ecu_reset                                  ( struct kwp_msg* msg ); /* 0x11 */
static enum kwp_response_code read_diagnostic_trouble_codes              ( struct kwp_msg* msg ); /* 0x13 */
static enum kwp_response_code clear_diagnostic_information               ( struct kwp_msg* msg ); /* 0x14 */
static enum kwp_response_code read_status_of_diagnostic_trouble_codes    ( struct kwp_msg* msg ); /* 0x17 */
static enum kwp_response_code read_diagnostic_trouble_codes_by_status    ( struct kwp_msg* msg ); /* 0x18 */
static enum kwp_response_code read_ecu_identification                    ( struct kwp_msg* msg ); /* 0x1A */
static enum kwp_response_code stop_diagnostic_session                    ( struct kwp_msg* msg ); /* 0x20 */
static enum kwp_response_code read_data_by_local_identifier              ( struct kwp_msg* msg ); /* 0x21 */
static enum kwp_response_code read_data_by_common_identifier             ( struct kwp_msg* msg ); /* 0x22 */
static enum kwp_response_code read_memory_by_address                     ( struct kwp_msg* msg ); /* 0x23 */
static enum kwp_response_code security_access                            ( struct kwp_msg* msg ); /* 0x27 */
static enum kwp_response_code write_data_by_common_identifier            ( struct kwp_msg* msg ); /* 0x2E */
static enum kwp_response_code start_routine_by_local_identifier          ( struct kwp_msg* msg ); /* 0x31 */
static enum kwp_response_code write_data_by_local_identifier             ( struct kwp_msg* msg ); /* 0x3B */
static enum kwp_response_code write_memory_by_address                    ( struct kwp_msg* msg ); /* 0x3D */
static enum kwp_response_code tester_present                             ( struct kwp_msg* msg ); /* 0x3E */

/*===============================================[ private variables ]================================================*/
static struct kwpservices_private kwpservices;


/** attention the list has to be in ascending order of SIDs */
const struct kwpserver_services kwpservices_table[]=
{
    { kwp_service_identifier_start_diagnostic_session                    , start_diagnostic_session                   } ,  /* 0x10 */
    { kwp_service_identifier_ecu_reset                                   , ecu_reset                                  } ,  /* 0x11 */
    { kwp_service_identifier_read_diagnostic_trouble_codes               , read_diagnostic_trouble_codes              } ,  /* 0x13 */
    { kwp_service_identifier_clear_diagnostic_information                , clear_diagnostic_information               } ,  /* 0x14 */
    { kwp_service_identifier_read_status_of_diagnostic_trouble_codes     , read_status_of_diagnostic_trouble_codes    } ,  /* 0x17 */
    { kwp_service_identifier_read_diagnostic_trouble_codes_by_status     , read_diagnostic_trouble_codes_by_status    } ,  /* 0x18 */
    { kwp_service_identifier_read_ecu_identification                     , read_ecu_identification                    } ,  /* 0x1A */
    { kwp_service_identifier_stop_diagnostic_session                     , stop_diagnostic_session                    } ,  /* 0x20 */
    { kwp_service_identifier_read_data_by_local_identifier               , read_data_by_local_identifier              } ,  /* 0x21 */
    { kwp_service_identifier_read_data_by_common_identifier              , read_data_by_common_identifier             } ,  /* 0x22 */
    { kwp_service_identifier_read_memory_by_address                      , read_memory_by_address                     } ,  /* 0x23 */
    { kwp_service_identifier_security_access                             , security_access                            } ,  /* 0x27 */
    { kwp_service_identifier_write_data_by_common_identifier             , write_data_by_common_identifier            } ,  /* 0x2E */
    { kwp_service_identifier_start_routine_by_local_identifier           , start_routine_by_local_identifier          } ,  /* 0x31 */
    { kwp_service_identifier_write_data_by_local_identifier              , write_data_by_local_identifier             } ,  /* 0x3B */
    { kwp_service_identifier_write_memory_by_address                     , write_memory_by_address                    } ,  /* 0x3D */
    { kwp_service_identifier_tester_present                              , tester_present                             } ,  /* 0x3E */
    { 0                                                                         , NIL                                        }    /* End of list */
};


static const U16 softwareVersion = 1;
static const U16 diagVersion= 1000;
static const U8 softwareModule =0x01;




/*================================================[ inline functions ]================================================*/

/*================================================[ public functions ]================================================*/


RC kwpservices_initialize( void )
{
   pal_memzero( &kwpservices, sizeof kwpservices);

   kwpservices.passive_mode=FALSE;
    secacc_initialize();

    return RC_SUCCESS;
}

BO kwpservices_get_passive_mode(void)
{
   return kwpservices.passive_mode;
}
/*===============================================[ private functions ]================================================*/
/**
*  KWP serviceFunction
*
*  start_diagnostic_session.
*
*   \return KWP_RESPONSE_CODE_SUCCESS if successful.
*   \return KWP_RESPONSE_CODE_INVALIDFORMAT/KWP_RESPONSE_CODE_SUBFUNCTIONNOTSUPPORTED
Request:
    $10(SID),DiagnosticSession(Par.1)
    DiagnosticSession: 
    $81: DefaultSession (MANDATORY for each ECU):
    - resets preceeding SecurityAccesses (see attached state flow)
    $85: ProgrammingSession (reserved for Bootblock)
Positive Response:
    $50(SID),DiagnosticSession(Par.1)
Negative Response:
    $7F(SID),$10(Par.1),ResponseCode(Par.2)
    Response Code
    $12
    SubFunctionNotSupported-InvalidFormat
    DiagnosticSession not supported
    wrong message format (e.g. data length)
Parameter DiagnosticSession according to ISO/DIS15765-3, 2000-04-27 
*/
static enum kwp_response_code start_diagnostic_session ( struct kwp_msg* msg ) /* 0x10 */
{
    enum kwp_response_code rv=KWP_RESPONSE_CODE_GENERALREJECT;


    if( msg->len != 0x02 ) /* check length */
    {
        msg->len = 3;
        rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
    }
    else
    {
        if( 0x81 == msg->data[1]) /* test diagnostic mode  */
        {
            secacc_reset_state();
            msg->len=2;
            rv=KWP_RESPONSE_CODE_SUCCESS;
        }
        else
        {
            rv=KWP_RESPONSE_CODE_SUBFUNCTIONNOTSUPPORTED;
        }
    }
    return rv;
}
/**
*  KWP serviceFunction
*
*  ecu_reset.
*   \return KWP_RESPONSE_CODE_SUCCESS if successful.
*   \return SubFunctionNotSupported-InvalidFormat
*   \return ConditionsNotCorrectOrReqSequenceError
*   \return SecurityAccessDenied
Request:
$11(SID),ResetMode(Par.1)
    ResetMode
    $01: PowerOn
    other values are not supported
Positive Response:
    $51(SID)
Negative Response:
    $7F(SID),$11(Par.1),ResponseCode(Par.2)
    Response Code
    $12
    SubFunctionNotSupported-InvalidFormat
    ResetMode not supported
    wrong message format (e.g. data length)
    $22
    ConditionsNotCorrectOrReqSequenceError
    PassiveMode not activated
    $33
    SecurityAccessDenied
    Function not allowed in actual SecurityState
    
Remark: 
1.
Function requires SecurityAccess3/4 or SecurityAccess5/6 (if supported) and PassiveMode.
Parameter ResetStatus is not supported. 
*/
static enum kwp_response_code ecu_reset( struct kwp_msg* msg ) /* 0x11 */
{
    enum kwp_response_code rv=KWP_RESPONSE_CODE_GENERALREJECT;
    
    if(( msg->len != 0x02 )||(0x01 != msg->data[1] ) )/* check length and ResetMode */
    {
        msg->len = 3;
        rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
    }
    else
    {   
        /*Valid kwp request format*/
        if(FALSE == kwpservices.passive_mode)                   /* check passive mode   */
        {
            msg->len = 3;
            rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
        }   
        else if(secacc_state_default != secacc_get_state()) /* check security state */
        {
            seppuku_perform(20,1);  /* delay 20*10ms */
            msg->len = 1;
            rv=KWP_RESPONSE_CODE_SUCCESS;
        }
        else
        {
            rv = KWP_RESPONSE_CODE_ACCESSDENIED;
        }
    }
    
    return rv;
}

/*
*   KWP serviceFunction
*   read_diagnostic_trouble_codes
*   \return KWP_RESPONSE_CODE_SUCCESS if successful.
*   \return SubFunctionNotSupported-InvalidFormat
*   \return RequestCorrectlyReceived-ResponsePending
Request:
    $13(SID),GroupOfDTC(Par.1)
    GroupOfDTC: unsigned8, values to be defined by device supplier  
    Linde conventions: 
    0x01: all active DTCs
    0x02: DTCs stored since last service
    0x03: all stored DTCs
Positive Response:
    $53(SID),NumberOfDTC(Par. 1),ListOfDTC=[DTC#1 ... DTC#m](Par. 2)
    NumberOfDTC: unsigned8
    DTC (DiagnosticTroubleCode): unsigned16, lowbyte first  
Negative Response:
    $7F(SID),$13(Par.1),ResponseCode(Par.2)
    Response Code
    $12
    SubFunctionNotSupported-InvalidFormat
    GroupOfDTC not supported
    wrong message format (e.g. data length)
    $78
    RequestCorrectlyReceived-ResponsePending
    Generation of response data has not yet been completed
*/
static enum kwp_response_code read_diagnostic_trouble_codes( struct kwp_msg* msg ) /* 0x13 */
{
    enum kwp_response_code rv=KWP_RESPONSE_CODE_GENERALREJECT;  

    if( msg->len != 0x02 ) /* check length */
    {
        msg->len = 3;
        rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
    }
    else
    {
        if(0x01 == msg->data[1])    /* all active DTCs */
        {
            U8 i = 0;
            U8 found_active_count = 0;
            msg->len = 2;
            found_active_count = 0;
            for(i = 0;i < ACTIVE_DTC_TOTAL_NUM;i++)
            {
                if(activeDTCbuffer[i] & ACTIVE_MASK)
                {
                    msg->len += 2;
                    msg->data[2+found_active_count*2] = (U8)(activeDTCbuffer[i]&0X00FF);
                    msg->data[2+found_active_count*2+1] =   (U8)((activeDTCbuffer[i]&DTCCODE_MASK)>>8);
                    ++found_active_count;
                }
                if(found_active_count == totalActive)
                {
                    break;
                }
            }
            msg->data[1] = totalActive;
            rv=KWP_RESPONSE_CODE_SUCCESS;
        }
        else if( (0x02 == msg->data[1]) || (0x03 == msg->data[1]))/* all stored DTCs */
        {           
            msg->len = 2;
            
            U8 i = 0;
            #if 0   /* read from EEPROM */
            /* 0- 99 */
            if(stReadWriteParam.total_historydtc_type0 <=  MAX_TOTAL_HISTORYDTC_TYPE0)
            {
                for(i = 0;i < stReadWriteParam.total_historydtc_type0;i++)
                {
                    U16 dtcContainer = 0;
                    sd_read( NVRAM_SD,&dtcContainer,&(pDtcCodeList->dtc_history_type0[(stReadWriteParam.pop_historydtcindex_type0+i)%MAX_TOTAL_HISTORYDTC_TYPE0]), 2);

                    if( (0 != (dtcContainer&ACTIVE_MASK) ) && ( (dtcContainer&DTCCODE_MASK) <= TYPE0_END) )
                    {
                        dtcContainer &= DTCCODE_MASK;
                        pal_memcpy( &msg->data[ msg->len], &dtcContainer, 2 );
                        msg->len += 2;
                    }
                }
            }
            /* 100- 199 */
            if(stReadWriteParam.total_historydtc_type1 <=  MAX_TOTAL_HISTORYDTC_TYPE1)
            {
                for(i = 0;i < stReadWriteParam.total_historydtc_type1;i++)
                {
                    U16 dtcContainer = 0;
                    sd_read( NVRAM_SD,&dtcContainer,&(pDtcCodeList->dtc_history_type1_code[(stReadWriteParam.pop_historydtcindex_type1+i)%MAX_TOTAL_HISTORYDTC_TYPE1]), 2);

                    if( (0 != (dtcContainer&ACTIVE_MASK) ) && ( (dtcContainer&DTCCODE_MASK) > TYPE0_END) && ( (dtcContainer&DTCCODE_MASK) <= TYPE1_END) )
                    {
                        dtcContainer &= DTCCODE_MASK;
                        pal_memcpy( &msg->data[ msg->len], &dtcContainer, 2 );
                        msg->len += 2;
                    }
                }
            }

            /* 200- 1023 */
            if(stReadWriteParam.total_historydtc_type2 <=  MAX_TOTAL_HISTORYDTC_TYPE2)
            {
                for(i = 0;i < stReadWriteParam.total_historydtc_type2;i++)
                {
                    U16 dtcContainer = 0;
                    
                    sd_read( NVRAM_SD,&dtcContainer,&(pDtcCodeList->dtc_history_type2_code[(stReadWriteParam.pop_historydtcindex_type2+i)%MAX_TOTAL_HISTORYDTC_TYPE2]), 2);

                    if( (0 != (dtcContainer&ACTIVE_MASK) ) && ( (dtcContainer&DTCCODE_MASK) > TYPE1_END) && ( (dtcContainer&DTCCODE_MASK) <= TYPE2_END) )
                    {
                        dtcContainer &= DTCCODE_MASK;
                        pal_memcpy( &msg->data[ msg->len], &dtcContainer, 2 );
                        msg->len += 2;
                    }
                }
            }
            msg->data[1] = stReadWriteParam.total_historydtc_type0 + stReadWriteParam.total_historydtc_type1 + stReadWriteParam.total_historydtc_type2;
            #else   /* read from RAM*/
            pal_memcpy(&msg->data[2], &historyDtcBuffer_Type0[0], stReadWriteParam.total_historydtc_type0*2);
            msg->len += stReadWriteParam.total_historydtc_type0*2;
            
            pal_memcpy(&msg->data[msg->len], &historyDtcBuffer_Type1[0], stReadWriteParam.total_historydtc_type1*2);
            msg->len += stReadWriteParam.total_historydtc_type1*2;
            
            pal_memcpy(&msg->data[msg->len],&historyDtcBuffer_Type2[0], stReadWriteParam.total_historydtc_type2*2);
            msg->len += stReadWriteParam.total_historydtc_type2*2;
            
            msg->data[1] = stReadWriteParam.total_historydtc_type0 + stReadWriteParam.total_historydtc_type1 + stReadWriteParam.total_historydtc_type2;

            /* clear non-dtc-code tag */
            U8 start = 0;
            for(start = 0; start < msg->data[1];start++)
            {
                U16* cursor = (U16*)(&msg->data[2 + start*2]);
                *cursor &= DTCCODE_MASK;
            }
            #endif
            
            rv=KWP_RESPONSE_CODE_SUCCESS;
        }
        else
        {
            msg->len = 3;
            rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
        }
        
    }
    
    return rv;
}
/*
*   KWP service Function
*   clear_diagnostic_information
*   \return KWP_RESPONSE_CODE_SUCCESS if successful.
*   \return SubFunctionNotSupported-InvalidFormat
*   \return RequestCorrectlyReceived-ResponsePending

Request:
    $14(SID),GroupOfDiagnosticInformation(Par.1)
    GroupOfDiagnosticInformation:
    a.Parameter GroupOfDiagnosticInformation (unsigned8),values to be defined by device supplier    
    b.Linde conventions: 
      0x02: DTCs stored since last service
      0x03: all stored DTCs
Positive Response:
    $54(SID),GroupOfDiagnosticInformation(Par. 1)
Negative Response:
    $7F(SID),$14(Par.1),ResponseCode(Par.2)
    Response Code:
    $12
    SubFunctionNotSupported-InvalidFormat
    GroupOfDiagnosticInfo not supported
    wrong message format (e.g. data length)
    $78
    RequestCorrectlyReceived-ResponsePending
    Function has not yet been completed
Remark:
1.
if this service is requested in case a SecurityAccess5/6 has been successfully performed, 
the complete history of DTCs is deleted physically in the Non Volatile Memory (NVM)
*/
static enum kwp_response_code clear_diagnostic_information( struct kwp_msg* msg ) /* 0x14 */
{

    enum kwp_response_code rv=KWP_RESPONSE_CODE_GENERALREJECT;  
    
    if( msg->len != 0x02 ) /* check length */
    {
        msg->len = 3;
        rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
    }
    else if(secacc_state_default == secacc_get_state()) /* check security state */
    {
        msg->len = 3;
        rv = KWP_RESPONSE_CODE_ACCESSDENIED;
    }
    else
    {
        if(0x01 == msg->data[1])    /* all active DTCs */
        {
            msg->len = 3;
            rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
        }
        else if( (0x02 == msg->data[1]) ||(0x03 == msg->data[1]) )/* all stored DTCs */
        {           
            if(reset_dtcStorageHandler())
            {
                msg->len = 2;
                rv=KWP_RESPONSE_CODE_SUCCESS;
            }
        }
        else
        {
            msg->len = 3;
            rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
        }
        
    }
    
    return rv;
}
/*
*   KWP service Function
*   read_status_of_diagnostic_trouble_codes
*   \return KWP_RESPONSE_CODE_SUCCESS if successful.
*   \return SubFunctionNotSupported-InvalidFormat
*   \return RequestCorrectlyReceived-ResponsePending
Request:
    $17(SID),GroupOfDTC(Par.1)
    GroupOfDTC: unsigned8, values to be defined by device supplier  
    Linde conventions: 
    0x02: DTCs stored since last service
    0x03: all stored DTCs
Posivive Respone:
    $57(SID),NumberOfDTC(Par.1),ListOfDTCAndStatus(Par.2)
    NumberOfDTC: unsigned8  
    ListOfDTCAndStatus=[DTC#1, StatusOfDTC#1, ... DTC#m, StatusOfDTC#m]
        DTC (DiagnosticTroubleCode): unsigned16 bit, lowbyte first
        StatusOfDTC depends of value of DTC:
            DTC = 0 ... 99: StatusOfDTC is not supported
            DTC = 100 ... 199: StatusOfDTC=[ TruckWorkTime, ComponentWorkTime, DateTime,ExtraData]
            DTC = 200 ... 999: StatusOfDTC=[ TruckWorkTime, ComponentWorkTime, DateTime,Length#1,Length#2,ExtraData#1,...ExtraData#n]
                TruckWorkTime:
                    unsigned32, lowbyte first, seconds since first power on
                    This data is generated by a particular ECU of the truck and sended cyclicaly on the CAN. In case of error detection the actual value is stored.
                ComponentWorkTime:
                    unsigned32, lowbyte first, seconds since first power on
                    Independant of the TruckWorkTime each ECU has ist own hourmeter. In case of error detection the actual value is stored.
                DateTime:
                    unsigned32, lowbyte first, seconds since 01.01.1970
                    An ECU with realtime clock sends this data cyclicaly on the CAN. In case of error detection the actual value is stored.
                ExtraData:
                    signed16, lowbyte first
                1.
                DTC-dependant value.
                Lenth#1:
                unsigned8
                lownibble: Number of ExtraData with format signed16 (valid values: n1 = 0...8)
                highnibble: Number of ExtraData with format float32 (valid values: n2 = 0...4)
                Additional restrictions:
                n = n1 + n2 = 0 �� 8
                2��n1 + 4��n2 = 0 �� 16
                Lenth#2:
                unsigned8
                reserved, always 0
                ExtraData#1 ... #n1:
                signed16, lowbyte first
                DTC-dependant value.
                ExtraData#(n1 + 1) ... #(n1 + n2):
                float32, lowbyte first
                DTC-dependant value.
Negative Response:
    $7F(SID),$17(Par.1),ResponseCode(Par.2)
    Response Code:
    $12
    SubFunctionNotSupported-InvalidFormat
    GroupOfDTC not supported
    wrong message format (e.g. data length)
    $78
    RequestCorrectlyReceived-ResponsePending
    Generation of response data has not yet been completed
*/
static enum kwp_response_code read_status_of_diagnostic_trouble_codes( struct kwp_msg* msg ) /* 0x17 */
{
    enum kwp_response_code rv=KWP_RESPONSE_CODE_GENERALREJECT;  

    if( msg->len != 0x02 ) /* check length */
    {
        msg->len = 3;
        rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
    }
    else
    {
        if(0x01 == msg->data[1])    /* all active DTCs */
        {
            msg->len = 3;
            rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
        }
        else if( (0x02 == msg->data[1]) || (0x03 == msg->data[1]) )/* all stored DTCs */
        {           
            msg->len = 2;
            
            U8 i = 0;
            /* 100- 199 */
            if(stReadWriteParam.total_historydtc_type1 <=  MAX_TOTAL_HISTORYDTC_TYPE1)
            {
                for(i = 0;i < stReadWriteParam.total_historydtc_type1;i++)
                {
                    U16 dtcContainer = 0;
                    sd_read( NVRAM_SD,&dtcContainer,&(pDtcCodeList->dtc_history_type1_code[(stReadWriteParam.pop_historydtcindex_type1+i)%MAX_TOTAL_HISTORYDTC_TYPE1]), 2);

                    if( (0 != (dtcContainer&ACTIVE_MASK) ) && ( (dtcContainer&DTCCODE_MASK) > TYPE0_END) && ( (dtcContainer&DTCCODE_MASK) <= TYPE1_END) )
                    {
                        /* read DTC CODE*/
                        dtcContainer &= DTCCODE_MASK;
                        pal_memcpy( &msg->data[ msg->len], &dtcContainer, 2 );
                        msg->len += 2;
                        /* read DTC STATUS*/
                        sd_read( NVRAM_SD,&msg->data[ msg->len],&(pDtcStatusList->dtc_history_type1_status[(stReadWriteParam.pop_historydtcindex_type1+i)%MAX_TOTAL_HISTORYDTC_TYPE1]), TYPE1_STATUS_LEN );
                        msg->len += TYPE1_STATUS_LEN;
                    }
                }
            }
            
            /* 200- 1023 */
            if(stReadWriteParam.total_historydtc_type2 <=  MAX_TOTAL_HISTORYDTC_TYPE2)
            {
                for(i = 0;i < stReadWriteParam.total_historydtc_type2;i++)
                {
                    U16 dtcContainer = 0;
                    sd_read( NVRAM_SD,&dtcContainer,&(pDtcCodeList->dtc_history_type2_code[(stReadWriteParam.pop_historydtcindex_type2+i)%MAX_TOTAL_HISTORYDTC_TYPE2]), 2);

                    if( (0 != (dtcContainer&ACTIVE_MASK) ) && ( (dtcContainer&DTCCODE_MASK) > TYPE1_END) && ( (dtcContainer&DTCCODE_MASK) <= TYPE2_END) )
                    {
                        /* read DTC CODE*/
                        dtcContainer &= DTCCODE_MASK;
                        pal_memcpy( &msg->data[ msg->len], &dtcContainer, 2 );
                        msg->len += 2;
                        /* read DTC STATUS*/
                        sd_read( NVRAM_SD,&msg->data[ msg->len],&(pDtcStatusList->dtc_history_type2_status[(stReadWriteParam.pop_historydtcindex_type2+i)%MAX_TOTAL_HISTORYDTC_TYPE2]), STATUS_SYS_INFO_LEN );
                        msg->len += STATUS_SYS_INFO_LEN;
                        U8 extraLen = 0;
                        /* Len1 */
                        sd_read( NVRAM_SD,&msg->data[ msg->len],&(pDtcStatusList->dtc_history_type2_status[(stReadWriteParam.pop_historydtcindex_type2+i)%MAX_TOTAL_HISTORYDTC_TYPE2].atom.Len1), 1 );
                        extraLen = msg->data[ msg->len];
                        extraLen = ( CNT16(extraLen)*2 + CNT32(extraLen)*4 );
                        msg->len += 1;
                        /* Len2*/
                        sd_read( NVRAM_SD,&msg->data[ msg->len],&(pDtcStatusList->dtc_history_type2_status[(stReadWriteParam.pop_historydtcindex_type2+i)%MAX_TOTAL_HISTORYDTC_TYPE2].atom.Len2), 1 );
                        msg->len += 1;
                        
                        if( extraLen > 16)
                        {
                            /* len error*/
                            extraLen = 16;
                        }
                        sd_read( NVRAM_SD,&msg->data[ msg->len],&(pDtcStatusList->dtc_history_type2_status[(stReadWriteParam.pop_historydtcindex_type2+i)%MAX_TOTAL_HISTORYDTC_TYPE2].atom.ExtraData), extraLen );
                        msg->len += extraLen;
                    }
                }
            }
            msg->data[1] = stReadWriteParam.total_historydtc_type1 + stReadWriteParam.total_historydtc_type2;
            rv=KWP_RESPONSE_CODE_SUCCESS;
        }
        else
        {
            msg->len = 3;
            rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
        }
        
    }
    
    return rv;
}
static enum kwp_response_code read_diagnostic_trouble_codes_by_status( struct kwp_msg* msg ) /* 0x18 */
{

    UNREFERENCED(msg);
    return KWP_RESPONSE_CODE_SUCCESS;
}
/*
1.
Request format:
$1A(SID),IdentificationOption(Par.1)
Positive Response:
$5A(SID),IdentificationRecordValue(Par.1)
Negative Response:
$7F(SID),$1A(Par.1),ResponseCode(Par.2)

2.
Request Par. 1
IdentificationOption:
$00 ... $7F: reserved by ISO 14230-3
$80: reserved for Linde HardwareID *), (see list of standardized system data)
$81: reserved for Linde SoftwareID#1, (see list of standardized system data)
$82: reserved for Linde SoftwareID#2 (string) currently not used
$83: reserved for Linde SoftwareID#3 (string) currently not used
$84: reserved for Linde SoftwareVersion, (see list of standardized system data)
$85: reserved for Linde ControllerType (string)
$86: reserved for Linde SoftwareID_Bootblock, (see list of standardized system data)
$87: reserved for Linde DiagVersion (see list of standardized system data)
$88: reserved for HardwareSerialNumber, (see list of standardized system data)
$89: reserved for Linde LastPartNumber *), (see list of standardized system data)
$8A: reserved for Linde SoftwareModule, (see list of standardized system data)
$8B...$8F: reserved for future use (to be defined by Linde)

IdentificationOption: values above $8F may be used for own purposes.
IdentificationRecord:
supported formats are
signed8 / unsigned8
signed16 / unsigned16, lowbyte first
signed32 / unsigned32, lowbyte first
string (max. 50 characters, transfer with first character first and without terminating 0x00)
Data, that can be accessed by this service, is integrated into the DiaMon Device Data Description.

3.
HardwareID, LastPartNumber and PartNumber have to have identical format
*/
static enum kwp_response_code read_ecu_identification( struct kwp_msg* msg ) /* 0x1A */
{
   U8 i;
   enum kwp_response_code rv=KWP_RESPONSE_CODE_GENERALREJECT;

   /*    Positive Response:   $5A,IdentificationRecordValue */
   /* Negative Response:   $7F,$1A,ResponseCode */
   if( msg->len != 0x02 ) /* check length */
   {
      rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
   }
   else
   {
      switch( msg->data[1])
      {
         case 0x80: /* hw id */
         {
            for(i = 0; i < 11; i++)
            {
                msg->data[i+1] = stEcuIdentifier.u8HW_Id[i] + 0x30;
            }
            msg->len=1+11;
            rv= KWP_RESPONSE_CODE_SUCCESS;
            break;
         }
         case 0x81: /*Linde SoftwareID#1 */
         {
            swidft_software_id_from_time( mcu_get_build_time_app(),(CH*) &msg->data[1] );
            msg->len=1+12;
            rv= KWP_RESPONSE_CODE_SUCCESS;
            break;
         }
         case 0x82: /*Linde SoftwareID#2 */
         {
            //rv= KWP_RESPONSE_CODE_SUCCESS;
            break;
         }
         case 0x83: /*Linde SoftwareID#3 */
         {
            //rv= KWP_RESPONSE_CODE_SUCCESS;
            break;
         }
         case 0x84: /*software version*/
         {
            endian_write_u16_little(&msg->data[1],  softwareVersion);
            msg->len=1+2;
            rv= KWP_RESPONSE_CODE_SUCCESS;
            break;
         }
         case 0x85: /*Linde Controller Type */
         {
            pal_memcpy( &msg->data[1],stEcuIdentifier.u8ControllerType, sizeof(stEcuIdentifier.u8ControllerType));
            msg->len=1+sizeof(stEcuIdentifier.u8ControllerType);
            rv= KWP_RESPONSE_CODE_SUCCESS;
            break;
         }
         case 0x86: /*Linde SoftwareID_Bootblock */
         {
            swidft_software_id_from_time( mcu_get_build_time_bb(),(CH*) &msg->data[1] );
            msg->len=1+12;
            rv= KWP_RESPONSE_CODE_SUCCESS;
            break;
         }
         case 0x87: /* Linde DiagVersion */
         {
            endian_write_u16_little(&msg->data[1], diagVersion );
            msg->len=1+2;
            rv= KWP_RESPONSE_CODE_SUCCESS;
            break;
         }
         case 0x88: /* HardwareSerialNumber */
         {
            pal_memcpy( &msg->data[1],&stEcuIdentifier.u8SerialNumber[0], 15);
            msg->len=1+15;
            rv= KWP_RESPONSE_CODE_SUCCESS;
            break;
         }
         case 0x89: /* Linde LastPartNumber */
         {
             for(i = 0; i < 11; i++)
             {
                 msg->data[i+1] = stEcuIdentifier.u8LastPartNumber[i] + 0x30;
             }
             msg->len=1+11;
             rv= KWP_RESPONSE_CODE_SUCCESS;
             break;
         }
         case 0x8A: /* sw-module */
         {
            msg->data[1]=softwareModule;
            msg->len=1+1;
            rv= KWP_RESPONSE_CODE_SUCCESS;
            break;
         }
         default:
         {
            break;
         }
      }
   }
   UNREFERENCED(msg);
   return rv;
}

static enum kwp_response_code stop_diagnostic_session ( struct kwp_msg* msg ) /* 0x20 */
{
   UNREFERENCED(msg);
   return KWP_RESPONSE_CODE_SUCCESS;
}



/*
Notice:
1.RecordLocalIdentifier:
$00: reserved for Linde PartNumber *) (see list of standardized system data)
$01: reserved for TruckSerialNumberEOL (see list of standardized system data)
$02: ComponentWorkTime [s] (unsigned32, read only) has to be supported by each ECU, that supports pathfinder-diagnostics
$03: BrandID (see list of standardized system data)
$04 ... $0F reserved for future use (to be defined by Linde)

2.HardwareID, LastPartNumber and PartNumber have to have identical format

3.
Request:
$21(SID),RecordLocalIdentifier(Par.1)
Positive Response:
$61(SID),RecordLocalIdentifier(Par.1),RecordValue(Par.2)
Negative Response:
$7F(SID),$21(Par.1),ResponseCode(Par.2)

4.
RecordValue:
supported formats are
signed8 / unsigned8
signed16 / unsigned16, lowbyte first
signed32 / unsigned32, lowbyte first
float32,
string (max. 50 characters, transfer with first character first and without terminating 0x00)
Bit-data can be mapped into unsigned8 and unsigned16.
Parameters TransmissionMode and MaximumNumberOfResponsesToSend are not supported.
Data, that can be accessed by this service, is integrated into the DiaMon Device Data Description.
*/
static enum kwp_response_code read_data_by_local_identifier( struct kwp_msg* msg ) /* 0x21 */
{
   enum kwp_response_code rv = KWP_RESPONSE_CODE_SUCCESS;
   U8 i;

   if( msg->len != 0x02 ) /* check length */
   {
      return KWP_RESPONSE_CODE_INVALIDFORMAT;
   }

   switch( msg->data[1])
   {
       case 0x00: /* partnumber */
       {
           for(i = 0; i< 11; i++)
           {
               msg->data[i+2] = stEcuIdentifier.u8PartNumber[i]+0x30;
           }
           msg->len +=11;
           break;
       }
       case 0x03: /* brand id */
       {
           pal_memcpy(&msg->data[2], &stEcuIdentifier.u16BrandID, 2 );
           msg->len +=2;
           break;
       }
       
       default:
       {
           rv =  parameter_read_kwp(msg->data[1],&msg->data[2], &msg->len, secacc_get_state());
           break;
       }
   }

   return rv;
}
/*
1.Don't use this service for Factory Parameters
remark:what kind of param is belongs to the so-called "Factory Parameters"?
2.
Request:
$22(SID),RecordCommonIdentifier(Par.1)
RecordCommonIdentifier (unsigned16, highbyte first):
$0000 ... $00FF reserved (don't use !!) $1000: reserved for TruckID (see list of standardized system data)
$FFE0 ... $FFEF reserved (LSP)
$FFF0 ... $FFFF reserved for future use (to be defined by Linde)

RecordValue:
supported formats are
signed8 / unsigned8
signed16 / unsigned16, lowbyte first
signed32 / unsigned32, lowbyte first
float32,
string (max. 50 characters, transfer with first character first and without terminating 0x00)
Bit-data can be mapped into unsigned8 and unsigned16.
Parameters TransmissionMode and MaximumNumberOfResponsesToSend are not supported.
Data, that can be accessed by this service, is integrated into the DiaMon Device Data Description.

Positive Response:
$62(SID),RecordCommonIdentifier(Par.1,unsigned16, highbyte first),RecordValue(Par.2)

Negative Response:
$7F(SID),$22(Par.1),ResponseCode

Response Code
$12
SubFunctionNotSupported-InvalidFormat
RecordCommonIdentifier not supported
wrong message format (e.g. data length)
$78
RequestCorrectlyReceived-ResponsePending
Generation of response data has not yet been completed
*/
static enum kwp_response_code read_data_by_common_identifier( struct kwp_msg* msg ) /* 0x22 */
{
    if( msg->len != 0x03 ) /* check length */
    {
        return KWP_RESPONSE_CODE_INVALIDFORMAT;
    }
    return parameter_read_kwp(endian_read_u16_big(&msg->data[1]),&msg->data[3], &msg->len, secacc_get_state());
}

/*
Notice:
1.
To be used for development only. These services should not be available for series devices.
*/
static enum kwp_response_code read_memory_by_address ( struct kwp_msg* msg ) /* 0x23 */
{
    if( 7!=msg->len )
    {
        return KWP_RESPONSE_CODE_INVALIDFORMAT;
    }

    {
      U32 addr = endian_read_u32_little(&msg->data[1]);
      U16 len  = endian_read_u16_little(&msg->data[5]);

      U8 device = len>>12;
      len&=0x0FFF;
      if(len>0x0FFE)
      {
         len=0x0FFE;
      }
      switch( device )
      {
         case 0x00: /* RAM */
         {
            pal_memcpy(&msg->data[1], (void*)addr, len);
            break;
         }
         case 0x01: /* EEPROM */
         {
            sd_read( NVRAM_SD , &msg->data[1],(void*) addr, len );
            break;
         }
         case 0x0F: /* test routine */
         {
            /* use address value as delay in ms to test pending message*/
            if( 0 == kwpservices.test_timer)
            {
               kwpservices.test_timer=systime_get();
            }

            if( systime_timeout(kwpservices.test_timer,addr ) )
            {
               U8 *p=&msg->data[1];
               U16 count=len;
               U8 value=0;
               while(count--)
               {
                  *p++=value++;
               }
               kwpservices.test_timer=0;
            }
            else
            {
               return KWP_RESPONSE_CODE_PENDING;

            }
         }
      }
        msg->len=len+1;
    }

    return KWP_RESPONSE_CODE_SUCCESS;
}


static enum kwp_response_code security_access( struct kwp_msg* msg ) /* 0x27 */
{
    U16 length = msg->len - 1;
    U8 rv = KWP_RESPONSE_CODE_SUCCESS;
    rv = secacc_process_request( kwpservices.passive_mode, &msg->data[1], &length);
    msg->len = length+1;
    return rv;
}

/* 
1.Don't use this service for Factory Parameters
2.Request:
$2E(SID),RecordCommonIdentifier(Par.1),Record Value(Par.2)

RecordCommonIdentifier (unsigned16, highbyte first):
$0000 ... $00FF reserved (don't use !!)
$1000: reserved for TruckID (read only, see list of standardized system data)
$FFE0 ... $FFEF reserved (LSP)
$FFF0 ... $FFFF reserved for future use (to be defined by Linde)

RecordValue:
supported formats are
signed8 / unsigned8
signed16 / unsigned16, lowbyte first
signed32 / unsigned32, lowbyte first
float32,
string (max. 50 characters, transfer with first character first and without terminating 0x00)
Bit-data can be mapped into unsigned8 and unsigned16.
Data, that can be accessed by this service, is integrated into the DiaMon Device Data Description.

Positive Response:
$6E(SID),RecordCommonIdentifier (unsigned16, highbyte first)

Negative Response:
$7F(SID),$2E(Par.1),ResponseCode(Par.2)

Response Code:
$12
SubFunctionNotSupported-InvalidFormat
RecordCommonIdentifier not supported
wrong message format (e.g. data length)
$31
RequestOutOfRange
Attempt to write invalid data value
$33
SecurityAccessDenied
Function not allowed in actual SecurityState
$78
RequestCorrectlyReceived-ResponsePending
Function has not yet been completed
*/
static enum kwp_response_code write_data_by_common_identifier( struct kwp_msg* msg ) /* 0x2E */
{
    RC rv=RC_SUCCESS;
    if( msg->len < 4)
    {
        return KWP_RESPONSE_CODE_INVALIDFORMAT;
    }

    rv = parameter_write_kwp( endian_read_u16_big(&msg->data[1]), &msg->data[3], msg->len-3, secacc_get_state() );

    if(rv != RC_SUCCESS)
    {
        msg->len = 3;
    }
    else
    {
        msg->len = 3;  
    }
    return rv;
}

/*
*   KWP Service Function - SID $31
*   start_routine_by_local_identifier
*   \return KWP_RESPONSE_CODE_SUCCESS if successful.
*   \return SubFunctionNotSupported-InvalidFormat
*   \return ConditionsNotCorrectOrReqSequenceError
*   \return RequestOutOfRange
*   \return SecurityAccessDenied
*   \return RequestCorrectlyReceived-ResponsePending

Request:
$31(SID),RoutineLocalIdentifier(Par.1),RoutineEntryOption(Par.2)
    Parameter RoutineLocalIdentifier (unsigned8) to address desired routine. 
    Parameter RoutineEntryOption (unsigned8 or unsigned16 (lowbyte first)) must be present(no matter if it is evaluated by addressed routine).
Positive Response:
    $71(SID),RoutineLocalIdentifier(Par.1)
Negative Response:
    $7F(SID),$31(Par.1),ResponseCode(Par.2)
    Response Code:
    $12
    SubFunctionNotSupported-InvalidFormat
    RoutineLocalIdentifier not supported
    RoutineEntryOption not supported
    wrong message format (e.g. data length)
    $22
    ConditionsNotCorrectOrReqSequenceError
    Function not allowed because its prerequisite conditions are not met (e.g. poti-teaching)
    Attempt to modify factory parameter while not in passive mode
    $31
    RequestOutOfRange
    Attempt to modify data to invalid value (e.g. increment/decrement)
    $33
    SecurityAccessDenied
    Function not allowed in actual SecurityState
    $78
    RequestCorrectlyReceived-ResponsePending
    Function has not yet been completed
Remark:
    1.RoutineLocalID/EntryOption reference table
    -----------------------------------------------------------------------------------------------------
    RoutineLocalID(unsigned8)   EntryOption(unsigned8)  description
    0x00                        all                     reserved for future use (to be defined by Linde)
    0x01                        0x00                    reserved for StartFlasherRoutine. Function requires SecurityAccess3/4 or SecurityAccess5/6 (if supported) and PassiveMode. MANDATORY for flashable ECUs
    0x02                        0x00                    reserved for ClearFlashMemoryRoutine (only Bootblock)
    0x03                        0x00                    reserved for ChecksumRoutine (only Bootblock)
    0x04                        0x00                    reserved for PassiveModeRoutine (physical and/or functional addressing).
                                                        In PassiveMode all CAN process communication is stopped (diagnosis communication is still possible), 
                                                        generation of errors concerning CAN process data is deactivated and all actuators are fixed in safe state.
                                                        Function requires SecurityAccess3/4 or SecurityAccess5/6 (if supported).
                                                        Activation of PassiveMode must be possible, if the controller is standalone (not part of a truck network, i.e. spare part production). MANDATORY for all ECUs
    0x37                        all                     reserved for production test                                                        
    0x80                        all                     reserved for DispatchRoutine (only Bootblock)
    0xF0                        0x00                    reserved for ParamsReset see below
    0xF1...0xFD                 all                     reserved for future use (to be defined by Linde)
    0xFE                        0x00                    reserved for ActuatorTestMode
    0xFF                        all                     reserved for future use (to be defined by Linde)
    Routine LocalID             EntryOption(unsigned16,lowbyte first)
    all                         0x0000...0x00FF         reserved
    all                         0xFFF0...0xFFFF         reserved
    --------------------------------------------------------------------------------------------------------
    2.Other values for RoutineLocalIdentifier and RoutineEntryOption to be defined by device manufacturer.
    3.Parameter RoutineEntryStatus is not supported.
    4.PassiveMode is required before modifying factory parameters to enforce an ECU reset before the changes take effect.!!!
    5.Linde convention for service parameters:!!!important to understand USE of RLID/RCID
    a.Read parameter value with 
    ReadDataByLID (RecordLocalIdentifier=RLID)or 
    ReadDataByCommonID (RecordCommonIdentifier=RCID) (MANDATORY for ECUs, that support service parameters)
    b.Write parameter (if desired) with 
      WriteDataByLID (RecordLocalIdentifier=RLID) or 
      WriteDataByCommonID (RecordCommonIdentifier=RCID)
    c.Modify parameter value (if desired) with StartRoutineByLID:
      RoutineLocalID    EntryOption             description
      0x10              RLID or RCID            increment service parameter with step#1
      0x11              RLID or RCID            increment service parameter with step#2
      0x12              RLID or RCID            decrement service parameter with step#1
      0x13              RLID or RCID            decrement service parameter with step#2
      0x18              RLID or RCID            set service parameter to its default value
      0xF0              0x00                    ParamsReset:
                                                reset all service parameters to their default values MANDATORY for ECUs, 
                                                that support service parameters
    d.
    RLID with format unsigned8
    RCID with format unsigned16, lowbyte first                                          
*/
static enum kwp_response_code start_routine_by_local_identifier( struct kwp_msg* msg ) /* 0x31 */
{
    enum kwp_response_code rv=KWP_RESPONSE_CODE_GENERALREJECT;
    
    if( ( msg->len != 0x03 ) && ( msg->len != 0x04 ))/* check length and ResetMode */   //Entry Option RLID(1 byte) or RCID 2 bytes
    {
        rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
    }
    else
    {
        switch(msg->data[1])
        {
            case 0x01:  /* reserved for PassiveModeRoutine */
            {
                if(0X00 == msg->data[2])    /* Active PassiveModeRoutine */
                {
                    if(FALSE == kwpservices.passive_mode)                   /* check passive mode   */
                    {
                        rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                    }   
                    else if(secacc_state_default == secacc_get_state()) /* check security state */
                    {
                        rv = KWP_RESPONSE_CODE_ACCESSDENIED;
                    }
                    else
                    {
                        deaddrop_p->stay_in_bb_flag = VALID_FLAG_VALUE;
                       seppuku_perform(20,1);  /* delay 20*10ms */
                         msg->len = 2;
                         rv = KWP_RESPONSE_CODE_SUCCESS;
                    }
                }
                break;
            }               
            case 0x04:  /* reserved for PassiveModeRoutine */
            {
                if(0X00 == msg->data[2])    /* Active PassiveModeRoutine */
                {
                    if(secacc_state_default == secacc_get_state()) /* check security state */
                    {
                        msg->len = 3;
                        rv = KWP_RESPONSE_CODE_ACCESSDENIED;
                    }
                    else
                    {
                        kwpservices.passive_mode = TRUE;
                        msg->len = 2;
                        rv = KWP_RESPONSE_CODE_SUCCESS;
                    }
                }
                break;
            }   
            case 0x10:/* increment service parameter with step#1 */
            {
                if(FALSE == kwpservices.passive_mode)                   /* check passive mode   */
                {
                    msg->len = 3;
                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                }   
                else if(secacc_state_default == secacc_get_state()) /* check security state */
                {
                    msg->len = 3;
                    rv = KWP_RESPONSE_CODE_ACCESSDENIED;
                }
                else
                {
                    // TO BE IMPLEMENT!
                    if(3 == msg->len)
                    {
                        //LID
                        switch(msg->data[2])    
                        {
                            case 0x00:
                                {
                                    //test 
                                    //LID increment service parameter with step#1
                                    //TO BE IMPLEMENT
                                    msg->len = 2;
                                    rv = KWP_RESPONSE_CODE_SUCCESS;
                                    break;
                                }
                            default:
                                {
                                    //unSupported LID
                                    
                                    msg->len = 3;
                                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                                }
                                
                        }
                    }
                    else if(4 == msg->len)
                    {
                        //CID
                        switch( endian_read_u16_big(&msg->data[2]) )
                        {
                            case 0x1122:    //litte-end or big-end to be verified
                                {
                                    //test 
                                    //CID increment service parameter with step#1
                                    msg->len = 2;
                                    rv = KWP_RESPONSE_CODE_SUCCESS;
                                    break;
                                }
                            default:
                                {
                                    //unSupported CID
                                    msg->len = 3;
                                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                                }
                                
                        }
                    }
                    
                }
                break;
            }
            case 0x11:/* increment service parameter with step#2 */
            {
                if(FALSE == kwpservices.passive_mode)                   /* check passive mode   */
                {
                    msg->len = 3;
                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                }   
                else if(secacc_state_default == secacc_get_state()) /* check security state */
                {
                    msg->len = 3;
                    rv = KWP_RESPONSE_CODE_ACCESSDENIED;
                }
                else
                {
                    // TO BE IMPLEMENT!
                    if(3 == msg->len)
                    {
                        //LID
                        switch(msg->data[2])    
                        {
                            case 0x00:
                                {
                                    //test 
                                    //LID increment service parameter with step#2
                                    //TO BE IMPLEMENT
                                    msg->len = 2;
                                    rv = KWP_RESPONSE_CODE_SUCCESS;
                                    break;
                                }
                            default:
                                {
                                    //unSupported LID
                                    
                                    msg->len = 3;
                                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                                }
                                
                        }
                    }
                    else if(4 == msg->len)
                    {
                        //CID
                        switch( endian_read_u16_big(&msg->data[2]) )
                        {
                            case 0x1122:    //litte-end or big-end to be verified
                                {
                                    //test 
                                    //CID increment service parameter with step#2
                                    msg->len = 2;
                                    rv = KWP_RESPONSE_CODE_SUCCESS;
                                    break;
                                }
                            default:
                                {
                                    //unSupported CID
                                    msg->len = 3;
                                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                                }
                                
                        }
                    }
                    
                }
                break;
            }
            case 0x12:/* decrement service parameter with step#1 */
            {
                if(FALSE == kwpservices.passive_mode)                   /* check passive mode   */
                {
                    msg->len = 3;
                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                }   
                else if(secacc_state_default == secacc_get_state()) /* check security state */
                {
                    msg->len = 3;
                    rv = KWP_RESPONSE_CODE_ACCESSDENIED;
                }
                else
                {
                    // TO BE IMPLEMENT!
                    if(3 == msg->len)
                    {
                        //LID
                        switch(msg->data[2])    
                        {
                            case 0x00:
                                {
                                    //test 
                                    //LID decrement service parameter with step#1
                                    //TO BE IMPLEMENT
                                    msg->len = 2;
                                    rv = KWP_RESPONSE_CODE_SUCCESS;
                                    break;
                                }
                            default:
                                {
                                    //unSupported LID
                                    
                                    msg->len = 3;
                                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                                }
                                
                        }
                    }
                    else if(4 == msg->len)
                    {
                        //CID
                        switch( endian_read_u16_big(&msg->data[2]) )    
                        {
                            case 0x1122:    //litte-end or big-end to be verified
                                {
                                    //test 
                                    //CID decrement service parameter with step#1
                                    msg->len = 2;
                                    rv = KWP_RESPONSE_CODE_SUCCESS;
                                    break;
                                }
                            default:
                                {
                                    //unSupported CID
                                    msg->len = 3;
                                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                                }
                                
                        }
                    }
                    
                }
                break;
            }           
            case 0x13:/* decrement service parameter with step#2 */
            {
                if(FALSE == kwpservices.passive_mode)                   /* check passive mode   */
                {
                    msg->len = 3;
                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                }   
                else if(secacc_state_default == secacc_get_state()) /* check security state */
                {
                    msg->len = 3;
                    rv = KWP_RESPONSE_CODE_ACCESSDENIED;
                }
                else
                {
                    // TO BE IMPLEMENT!
                    if(3 == msg->len)
                    {
                        //LID
                        switch(msg->data[2])    
                        {
                            case 0x00:
                                {
                                    //test 
                                    //LID decrement service parameter with step#1
                                    //TO BE IMPLEMENT
                                    msg->len = 2;
                                    rv = KWP_RESPONSE_CODE_SUCCESS;
                                    break;
                                }
                            default:
                                {
                                    //unSupported LID
                                    
                                    msg->len = 3;
                                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                                }
                                
                        }
                    }
                    else if(4 == msg->len)
                    {
                        //CID
                        switch( endian_read_u16_big(&msg->data[2]) )
                        {
                            case 0x1122:    //litte-end or big-end to be verified
                                {
                                    //test 
                                    //CID decrement service parameter with step#1
                                    msg->len = 2;
                                    rv = KWP_RESPONSE_CODE_SUCCESS;
                                    break;
                                }
                            default:
                                {
                                    //unSupported CID
                                    msg->len = 3;
                                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                                }
                                
                        }
                    }
                    
                }
                break;
            }           
            case 0x18:  /* set service parameter to its default value */
            {
                if(FALSE == kwpservices.passive_mode)                   /* check passive mode   */
                {
                    msg->len = 3;
                    rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                }   
                else if(secacc_state_default == secacc_get_state()) /* check security state */
                {
                    msg->len = 3;
                    rv = KWP_RESPONSE_CODE_ACCESSDENIED;
                }
                else
                {
                    switch(endian_read_u16_big(&msg->data[1]))
                    {
                    default: break;
                    }
                }
                break;
            }
            case 0xF0:  /* ParamsReset */
            {
                if(0X00 == msg->data[2])    /* Active PassiveModeRoutine */
                {
                    if(FALSE == kwpservices.passive_mode)                   /* check passive mode   */
                    {
                        msg->len = 3;
                        rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
                    }   
                    else 
                    if(secacc_state_default == secacc_get_state()) /* check security state */
                    {
                        msg->len = 3;
                        rv = KWP_RESPONSE_CODE_ACCESSDENIED;
                    }
                    else
                    {
                        /*trigger ParamsReset */
						pal_memcpy( service_obj.ram_param, service_obj.default_parameter, (pal_size_t) service_obj.parameter_struct_size  ); 
						if(RC_SUCCESS == paragen_store(&service_obj))
						{
	                        msg->len = 2;
	                        rv = KWP_RESPONSE_CODE_SUCCESS;
						}
						else
						{
							msg->len = 3;
							rv = KWP_RESPONSE_CODE_ROUTINENOTCOMPLETED;
						}
                    }
                }
                break;
            }                           
            default:
            {
                U16 RespExt=ApplDiagRequest( msg->data[1], RM_FROMKWP);
                switch (RespExt)
                {
                    case KWP_RESPONSE_CODE_SUCCESS:
                        msg->len = 2;
                        rv = RespExt;
                        break;
                    default:          
                        msg->len = 3;
                        rv = RespExt;
                        break;
                }
                break;
            }
        }
    }

    return rv;
}

/*
Notice:
1.Writing of factory parameters requires a preceding SecurityAccess5/6. 
factory parameters must be of format signed16 or unsigned16. 
PassiveMode (see StartRoutineByLocalIdentifier) is required 
before modifying factory parameters to enforce an ECU reset before the changes take effect

2.
Request:
$3B(SID),RecordLocalIdentifier(Par.1),Record Value(Par.2)

RecordLocalIdentifier:
$00: reserved for Linde PartNumber *) (see list of standardized system data)
$01: TruckSerialNumberEOL (see list of standardized system data)
$02: writing of ComponentWorkTime is not supported
$03: BrandID (see list of standardized system data)
$04 ... $0F reserved for future use (to be defined by Linde)
*) HardwareID, LastPartNumber and PartNumber have to have identical format

RecordValue:
supported formats are
signed8 / unsigned8
signed16 / unsigned16, lowbyte first
signed32 / unsigned32, lowbyte first
float32,
string (max. 50 characters, transfer with first character first and without terminating 0x00)
Bit-data can be mapped into unsigned8 and unsigned16.
Data, that can be accessed by this service, is integrated into the DiaMon Device Data Description.

Positive Respone:
$7B(SID),RecordLocalIdentifier(Par.1)

Negative Response:
$7F(SID),$3B(Par.1),ResponseCode(Par.2)

*/
static enum kwp_response_code write_data_by_local_identifier( struct kwp_msg* msg ) /* 0x3B */
{
    enum kwp_response_code rv=KWP_RESPONSE_CODE_SUCCESS;
    U8 i;

    if( msg->len < 3 )
    {
        return KWP_RESPONSE_CODE_INVALIDFORMAT;
    }

    switch( msg->data[1])
    {
        case 0x00: /* partnumber */
        {
            pal_memcpy(stEcuIdentifier.u8LastPartNumber, stEcuIdentifier.u8PartNumber, 12);
            for(i = 0; i < msg->len-2; i++)
            {
                stEcuIdentifier.u8PartNumber[i] = msg->data[i+2] - 0x30;
            }
            sd_write(NVRAM_SD, (U8 *)(&nvdata->stIdentifier) + 0x0e, (U8*)&stEcuIdentifier.u8PartNumber[0],msg->len - 2);
            sd_write(NVRAM_SD, (U8 *)(&nvdata->stIdentifier) + 0x1A, (U8*)&stEcuIdentifier.u8LastPartNumber[0], 11);
            break;
        }
        default:
        {
            rv =  parameter_write_kwp( msg->data[1], &msg->data[2], msg->len-2, secacc_get_state() );
            break;
        }
    }
    
    if(rv != KWP_RESPONSE_CODE_SUCCESS)
    {
        msg->len = 3;
    }
    else
    {
        msg->len = 2;
    }
    return rv;
}

/*
Notice:
1.
To be used for development only. These services should not be available for series devices.
*/
static enum kwp_response_code write_memory_by_address( struct kwp_msg* msg ) /* 0x3D */
{

    UNREFERENCED(msg);
    return KWP_RESPONSE_CODE_SUCCESS;
}

static enum kwp_response_code tester_present( struct kwp_msg* msg ) /* 0x3E */
{

    if ( 2 != msg->len )
    {
        msg->len = 3;
        return KWP_RESPONSE_CODE_INVALIDFORMAT;
    }

    switch ( msg->data[1] )
    {
        case 0x01:
        {
            msg->len = 1; /* return sid */
            return KWP_RESPONSE_CODE_SUCCESS;
        }
        case 0x02:
        {
            return  KWP_RESPONSE_CODE_NONE;
       }
       default:
       {
           msg->len = 3;
           return KWP_RESPONSE_CODE_SUBFUNCTIONNOTSUPPORTED;
       }
    }
}

/*
input parmeters
dtc_code        extra                       extraLen
0..99       null                                 0      ----no extra information
100..199        null or valid_pointer                0 or 1 ----1 s16 information
200..999        null or valid_pointer                0 or 8 ----8 s16 information
*/
S8 DTC_trigger_with_extraInformation(U16 dtc_code,S16 extra[],U8 extraLen)
{

    return 1;
}

/***********************************************************************************************************************
*                             Copyright 2017 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/
